from robot_arm import RobotArm
from camera import Camera
from claw import claw
from threading import *
import math
import util_3d as u3d
from spatialmath.base import *

dock1    = [0.423434455160897, 0.5277398873066397, 0.1480215307405189, -1.571309267801182, -0.0070136847519165135, 1.5680461841395927]
dock1_up = [0.4233964867837777, 0.5277762254304993, 0.41431757198138014, -1.5713450967379636, -0.006964817856513663, 1.568224802036512]
ready    = [0.17248050865288567, 0.7129786706606164, 0.4215339761560544, -1.5719438886087616, 0.0002524637255009545, 0.0008128762716340789]

ready2    = [0.17248050865288567, 0.6879786706606164, 0.4215339761560544, -1.5719438886087616, 0.0002524637255009545, 0.0008128762716340789]
ready3    = [0.17248050865288567, 0.73779786706606164, 0.4215339761560544, -1.5719438886087616, 0.0002524637255009545, 0.0008128762716340789]
ready4    = [0.17248050865288567, 0.4629786706606164, 0.4215339761560544, -1.5719438886087616, 0.0002524637255009545, 0.0008128762716340789]


alpha = 0.05 / 1147.0

cr12 = RobotArm()
ca = Camera(camera_name='U3V:DA3797248 MV-CU060-10UM')
ca.set_math_template()
ca.set_param_outer('./param_outer_agv.txt')
# ca.set_param()
# ca.set_param_outer('param_outer_agv.txt')
t1 = Thread(target=ca.update_image_loop)
t1.start()


base_t_end_place0 = u3d.rokae_pose_to_T(dock1)
base_t_end_obs = u3d.rokae_pose_to_T(ready)
base_t_cam = u3d.halcon_pose_to_T(ca.out_param_halcon_type)
cam_t_pan_obs0 = u3d.rokae_pose_to_T([0.0009462006818965517, 0.0026534484482193016, 0.29, 0, 0, 0.005690707783380899])
cam_t_pan_obs_i = u3d.rokae_pose_to_T([0.0018287793689237576, -0.000381746410506795, 0.29, 0.0, 0, 0.007004064885668813])
base_t_end_place_i = u3d.rokae_pose_to_T(dock1)
A = base_t_end_place0@ trinv(base_t_end_obs)@ base_t_cam @ cam_t_pan_obs0
B = trinv(base_t_end_obs)@base_t_cam
rad2deg = 180/math.pi

def dis(a,b):
    if len(a) == len(b):
        for i in range(len(a)):
            print('error', i, a[i] - b[i])

def process_place_pan_to_dock():
    pass
def process_move_from_dock_obs():
    pass
def calu_angle(a,b,c):
    tmp = (a*a+b*b-c*c)/(2*a*b)
    return math.acos(tmp)

def process_calu_theta():
    cr12.movel_rokae(ready)
    flag, re1 = ca.circle_detect()
    # 注意X,Y的顺序
    Y = (re1[0] - 1024)*alpha
    X = (re1[1] - 1536)*alpha
    print('point1', re1)
    cr12.movel_rokae(ready4)
    flag, re2 = ca.circle_detect()
    # if flag == False:
    #     exit()
    print('point2',re2)
    du = re2[0]-re1[0]
    dv = re2[1]-re1[1]
    dist = math.sqrt(du*du+dv*dv)
    print('dist', du, dv, dist)
    a = 0.25
    b = 0.25
    c = dist*alpha
    theta = calu_angle(a,b,c)
    if dv < 0:
        theta = -theta
    print('obs',X, Y, c, theta) # m m rad
    cam_t_pan_obs_i = u3d.rokae_pose_to_T([X, Y, 0.29, 0, 0, theta])
    base_t_end_place_i =  A@trinv(cam_t_pan_obs_i)@ trinv(B)
    base_t_end_place_i_rokae = u3d.T_to_rokae_pose(base_t_end_place_i)
    dis(base_t_end_place_i_rokae, dock1)
    dock1_tmp_up = base_t_end_place_i_rokae[:]
    dock1_tmp_up[2] = base_t_end_place_i_rokae[2] + 0.27
    cr12.movel_rokae(dock1_tmp_up)
    cr12.movel_rokae(base_t_end_place_i_rokae)
    tmp_A = base_t_end_place_i @ trinv(base_t_end_obs) @ base_t_cam @ cam_t_pan_obs_i
    print(A)


def process_calu_theta_i():
    cr12.movel_rokae(ready)
    flag, re3 = ca.circle_detect()
    Y = (re3[0] - 1024) * alpha
    X = (re3[1] - 1536) * alpha
    print(re3)
    cr12.movel_rokae(ready4)
    flag, re2 = ca.circle_detect()
    print(re2)
    dx = re2[0] - re3[0]
    dy = re2[1] - re3[1]
    dist = math.sqrt(dy * dy + dx * dx)
    print(dx, dy, dist)
    a = 0.25
    b = 0.25
    c = dist * alpha
    theta = calu_angle(a, b, c)
    print(X, Y, theta)  # m m rad
    cam_t_pan_obs_i = u3d.rokae_pose_to_T([X, Y, 0.29, 0, 0, theta])
    base_t_end_place_i = A@trinv(cam_t_pan_obs_i)@trinv(B)


    # 0.0018950549193318604 - 0.012657061752051776 0.39905002521894983

# def claw_jia
#
# def calu_correct()
#     A =
#     B =
def process_tmp():
    cr12.movel_rokae(dock1_up)
    cr12.movel_rokae(ready)

def process_tmp3():
    cr12.movel_rokae(dock1)
    claw(True)
    cr12.movel_rokae(dock1_up)
    cr12.movel_rokae(ready)

def process_tmp4():
    # claw(False)
    cr12.movel_rokae(ready)
    cr12.movel_rokae(dock1_up)
    cr12.movel_rokae(dock1)
    while True:
         print('11')
    claw(True)
    cr12.movel_rokae(dock1_up)
    cr12.movel_rokae(ready)

def process_record():
    while True:
        pose_rokea = cr12.get_end_rokae_type()
        print(pose_rokea)
        input()
def process_calib_record_cam_t_pan_obs():
    cr12.movel_rokae(ready)
    flag, re1 = ca.circle_detect()
    # 注意X,Y的顺序
    Y = (re1[0] - 1024) * alpha
    X = (re1[1] - 1536) * alpha
    print('point1', re1)
    cr12.movel_rokae(ready4)
    flag, re2 = ca.circle_detect()
    # if flag == False:
    #     exit()
    print('point2', re2)
    du = re2[0] - re1[0]
    dv = re2[1] - re1[1]
    dist = math.sqrt(du * du + dv * dv)
    print('dist', du, dv, dist)
    a = 0.25
    b = 0.25
    c = dist * alpha
    theta = calu_angle(a, b, c)
    if dv < 0:
        theta = -theta
    print([X, Y, 0.29, 0, 0, theta])


if __name__ == '__main__':
    # process_calu_theta()
    # process_tmp4()
    # process_record()
    # process_record_cam_t_pan_obs()
    print(ca.circle_detect())
    # claw(False)
    # claw(False)
    # claw(True)
    # claw(True)
    # process_calu_theta()
    # process_tmp()
    # while True:
    #     a = input()
    #     pose_rokea = cr12.get_end_rokae_type()
    #     print(pose_rokea)

    # print(cr12.is_arrival(dock1_up))
    # print(cr12.is_arrival(dock1))

    # pose_rokea[3] = - 3.141592/2
    # pose_rokea[4] = 0
    # claw(False)

    # cur_T = cr12.get_end_T()
    # delta_T = xyzrpy_to_T(-0.1,0.0,0.0,0.0,0.0,0.0)
    # cur_T = delta_T @ cur_T
    # cr12.movel_to_T(cur_T)